/**
 * @file laser_rangefinder_node.cpp
 * @author yz 
 * @brief laser_rangefinder node
 * @version 0.1
 * @date 2021-11-19
 * 
 * @copyright Copyright (c) 2021
 * 
 */


#include"laser_rangefinder_node.h"
#include"laser_rangefinder/laser_rangefinder.h"



int main(int argc,char** argv)
{
    //初始化
    ros::init(argc,argv,"LASER_RANGEFINDER");

    ros::NodeHandle serial("~");

    ros::Publisher send=serial.advertise<laser_rangefinder::laser_rangefinder>("distance",1000);

    ros::Rate loop_rate(250);
    // ros::spinOnce();

    laser_rangefinder::laser_rangefinder msg;

    serial::Serial sp;
    serial::Timeout to = serial::Timeout::simpleTimeout(100);

    std::string port;
  
  sp.setPort("/dev/ttyUSB0");

  sp.setBaudrate(115200);
  sp.setTimeout(to);

  try
  {
      sp.open();
  }
  catch(serial::IOException& e)
  {
      ROS_INFO("Unable to open port!!!");
      return -1;
  }
  if(sp.isOpen())
  {
      ROS_INFO("the USB is opened.");

  }
   else
  {
        return -1;  
  }

    char cmd[]="\xDA\x00\x00\x16\x16\xEE";

    // BufferPrintf((u_int8_t*)&cmd);
    uint8_t buffer[1024];

    while (ros::ok())
    {

        size_t n = sp.available();      
        if(n!=0)
        {
           n = sp.read(buffer,n);
           for (int i=0;i<n;++i)
           {
                 16进制的方式打印到屏幕
                cout<<std::hex<<(buffer[i]&0xff)<<"  ";
                cout<<endl;           
           }
            
            if(6 == n)
            {
                
                if ((uint16_t)buffer[0]==0xda){
                    if((uint16_t)buffer[5]==0xee){

                    double high = (double)((uint16_t)buffer[1]*256+ (uint16_t)buffer[2]);
                    double low  = (double)(((uint16_t)buffer[3]*256+ (uint16_t)buffer[4]));
                    low = low/65535;
                    // cout<<"low is "<<low<<endl;
                    msg.the_distance =high+low;
                    if (high == 39321)
                    {
                        cout<<"激光测距仪被遮挡"<<endl;
                        // continue;
                    }
                    else{
                        printf("the distance  is %5.3f m\n",msg.the_distance);

                    }
                    
                    send.publish(msg);

                  }
                }

            }

            memset(buffer,0x00,sizeof(buffer));

         }

        loop_rate.sleep();
        
    }

}
